#include "drv_SetPlanning.h"
#include "drv_utils.h"

void SetPlanning_OldCal(SetPlanning_Str *Str)
{
    // 一些中间变量
    float Accl_Cal;

    // 记录计算时刻，仅供Debug
    Str->CalTick = rt_tick_get();

    // 对输出的位置和速度按照实际状态进行限幅
    if (Str->Output.pos - Str->Input.Now.pos > Str->Settings.POS_Error_Max)
    {
        Str->Output.pos = Str->Input.Now.pos + Str->Settings.POS_Error_Max;
        // Str->Output.spe = 0.f;
    }
    else if (Str->Output.pos - Str->Input.Now.pos < -Str->Settings.POS_Error_Max)
    {
        Str->Output.pos = Str->Input.Now.pos - Str->Settings.POS_Error_Max;
        // Str->Output.spe = 0.f;
    }

    Str->Temp.DeltaSpe = Str->Input.Set.spe - Str->Output.spe;
    Str->Temp.DeltaSpe_2 = Str->Temp.DeltaSpe * Str->Temp.DeltaSpe;
    Str->Temp.DeltaPos = Str->Input.Set.pos - Str->Output.pos;
    Str->Temp.DeltaPos_Temp = Str->Temp.DeltaSpe_2 / (2 * Str->Settings.Accl_Max * 0.95f); // 以最大加速度将速度减为零所用的位移（减速位移）

    if (Str->Temp.DeltaPos * Str->Temp.DeltaSpe < 0) // 当前位移增量和速度增量的方向相反
    {
        if (Str->Temp.DeltaPos > 0)
        {
            if (Str->Temp.DeltaPos * 0.9f > Str->Temp.DeltaPos_Temp)
            {
                Accl_Cal = Str->Settings.Accl_Max; // 当前位置和减速位移还有一定距离,则依然加速
            }
            else
            {
                Accl_Cal = -Str->Settings.Accl_Max; // 到该减速的位置了，则减速
            }
        }
        else
        {
            if (-Str->Temp.DeltaPos * 0.9f > Str->Temp.DeltaPos_Temp) // 原理同上
            {
                Accl_Cal = -Str->Settings.Accl_Max;
            }
            else
            {
                Accl_Cal = Str->Settings.Accl_Max;
            }
        }
    }
    else
    {
        if (Str->Temp.DeltaPos > 0)
        {
            Accl_Cal = Str->Settings.Accl_Max;
        }
        else
        {
            Accl_Cal = -Str->Settings.Accl_Max;
        }
    }
    Str->Temp.Accl_Now = Accl_Cal;
    Str->Temp.OutputTemp.spe = Str->Temp.Accl_Now * Str->Settings.dt + Str->Output.spe; // 计算规划后的速度
    /*对规划后的速度进行限幅*/
    if (Str->Temp.OutputTemp.spe > Str->Settings.Speed_Max)
    {
        Str->Temp.OutputTemp.spe = Str->Settings.Speed_Max;
    }
    else if (Str->Temp.OutputTemp.spe < -Str->Settings.Speed_Max)
    {
        Str->Temp.OutputTemp.spe = -Str->Settings.Speed_Max;
    }

    if ((fabsf(Str->Temp.DeltaPos) > fabsf(Str->Output.spe * Str->Settings.dt)) ||
        (fabsf(Str->Temp.DeltaSpe) > 2 * Str->Settings.Accl_Max * Str->Settings.dt))
    {
        // 速度增量或位置增量都大于从静止状态加速一个周期的距离和速度时，正常设定值规划的输出
        Str->Output.spe = Str->Temp.OutputTemp.spe;
        Str->Output.pos += Str->Output.spe * Str->Settings.dt;
    }
    else
    {
        // 剩余增量很小，可以忽略时，则不进行设定值规划，直出设定值
        Str->Output.pos = Str->Input.Set.pos;
        Str->Output.spe = Str->Input.Set.spe;
    }
}

void SetPlanning_Cal(SetPlanning_Str *Str)
{
    // 一些中间变量
    float Accl_Cal;

    // 记录计算时刻，仅供Debug
    Str->CalTick = rt_tick_get();

    // 对输出的位置和速度按照实际状态进行限幅
    if (Str->Output.pos - Str->Input.Now.pos > Str->Settings.POS_Error_Max)
    {
        Str->Output.pos = Str->Input.Now.pos + Str->Settings.POS_Error_Max;
        // Str->Output.spe = 0.f;
    }
    else if (Str->Output.pos - Str->Input.Now.pos < -Str->Settings.POS_Error_Max)
    {
        Str->Output.pos = Str->Input.Now.pos - Str->Settings.POS_Error_Max;
        // Str->Output.spe = 0.f;
    }

    Str->Temp.DeltaSpe = Str->Input.Set.spe - Str->Output.spe;
    Str->Temp.DeltaSpe_2 = fabsf(Str->Input.Set.spe * Str->Input.Set.spe - Str->Output.spe * Str->Output.spe);
    Str->Temp.DeltaPos = Str->Input.Set.pos - Str->Output.pos;
    Str->Temp.DeltaPos_Temp = Str->Temp.DeltaSpe_2 / (2 * Str->Settings.Accl_Max * 0.95f); // 以最大加速度将速度减为零所用的位移（减速位移）

    if (Str->Temp.DeltaPos * Str->Temp.DeltaSpe < 0) // 当前位移增量和速度增量的方向相反
    {
        if (Str->Temp.DeltaPos > 0)
        {
            if (Str->Temp.DeltaPos * 0.9f > Str->Temp.DeltaPos_Temp)
            {
                Accl_Cal = Str->Settings.Accl_Max; // 当前位置和减速位移还有一定距离,则依然加速
            }
            else
            {
                Accl_Cal = -Str->Settings.Accl_Max; // 到该减速的位置了，则减速
            }
        }
        else
        {
            if (-Str->Temp.DeltaPos * 0.9f > Str->Temp.DeltaPos_Temp) // 原理同上
            {
                Accl_Cal = -Str->Settings.Accl_Max;
            }
            else
            {
                Accl_Cal = Str->Settings.Accl_Max;
            }
        }
    }
    else
    {
        if (Str->Temp.DeltaPos > 0)
        {
            Accl_Cal = Str->Settings.Accl_Max;
        }
        else
        {
            Accl_Cal = -Str->Settings.Accl_Max;
        }
    }
    Str->Temp.Accl_Now = Accl_Cal;
    Str->Temp.OutputTemp.spe = Str->Temp.Accl_Now * Str->Settings.dt + Str->Output.spe; // 计算规划后的速度
    /*对规划后的速度进行限幅*/
    if (Str->Temp.OutputTemp.spe > Str->Settings.Speed_Max)
    {
        Str->Temp.OutputTemp.spe = Str->Settings.Speed_Max;
    }
    else if (Str->Temp.OutputTemp.spe < -Str->Settings.Speed_Max)
    {
        Str->Temp.OutputTemp.spe = -Str->Settings.Speed_Max;
    }

    if ((fabsf(Str->Temp.DeltaPos) > fabsf(Str->Temp.OutputTemp.spe * Str->Settings.dt)) &&
        (fabsf(Str->Temp.DeltaSpe) > Str->Settings.Accl_Max * Str->Settings.dt))
    {
        // 速度增量或位置增量都大于从静止状态加速一个周期的距离和速度时，正常设定值规划的输出
        Str->Output.spe = Str->Temp.OutputTemp.spe;
        Str->Output.pos += Str->Output.spe * Str->Settings.dt;
    }
    else
    {
        // 剩余增量很小，可以忽略时，则不进行设定值规划，直出设定值
        Str->Output.pos = Str->Input.Set.pos;
        Str->Output.spe = Str->Input.Set.spe;
    }
}

#define SET_sign(x) ((x)>0?1:-1)
void SetPlanning_NewCal(SetPlanning_Str *Str)
{
    // 一些中间变量
    float Tb, Tm, Vm, Te;
    float Accl_Cal;

#define Sn Str->Output.pos
#define St Str->Input.Set.pos
#define Vn Str->Output.spe
#define Vt Str->Input.Set.spe
#define a Str->Settings.Accl_Max

    Vt = Vt > Str->Settings.Speed_Max ? Str->Settings.Speed_Max : Vt; // 输入限幅
    Vt = Vt < -Str->Settings.Speed_Max ? -Str->Settings.Speed_Max : Vt;

	
    // 计算加速时间Tb和减速时间Te
    Tb = ((Vt - Vn) / a + 2 * (Sn - St) / (3 * Vt + Vn)) / (4 * Vt / (Vn + 3 * Vt));
    Te = (2 * (Sn - St) + Tb * (Vn - Vt)) / (Vt + 3 * Vn);
    Tm = 0;
		Vm = SET_sign(Tb) * Str->Settings.Speed_Max;

    // 计算输出结果
    if (fabs(Vn + Tb * a) > Str->Settings.Speed_Max)
    {
        Tb = (Vm - Vn) / a;
        Te = (Vn + Tb * a - Vt) / a;
        Tm = (Sn - St + (Vt * Vt - Vn * Vn) / (2 * a) - Vt * (Tb + Te)) / (Vt - Vm);
    }

    // if(fabs(Tb) > fabs(Te))
    if (Tb > 0)
        Accl_Cal = Str->Settings.Accl_Max;
    else if (Tb < 0)
        Accl_Cal = -Str->Settings.Accl_Max;
    else if (Tb == 0)
        Accl_Cal = 0;

    /*对规划后的输出进行限幅*/
//    if ((fabsf(Tb) + fabsf(Te) + fabsf(Tm)) < Str->Settings.dt)
//    {
//        //说明可以达到目标
//        Sn = St;
//        Vn = Vt;
//    } 
//    else if ((fabsf(Tb) + fabsf(Tm)) < Str->Settings.dt && Tm != 0)
//    {
//        //说明要越过横加速阶段，先计算剩余时间
//        float remain_time = fabsf(Te) - Str->Settings.dt + (fabsf(Tb) + fabsf(Tm));
//        Vn = Vt - remain_time * SET_sign(Te) * a;
//        Sn = St - (Vt * Vt - Vn * Vn) / 2 / a;
//    }
//    else if (Tm < Str->Settings.dt && Tm != 0)
//    {
//        //直接进入减速阶段
//        float remain_time = fabsf(Te) - Str->Settings.dt + fabsf(Tm);
//        Vn = Vt - remain_time * SET_sign(Te) * a;
//        Sn = St - (Vt * Vt - Vn * Vn) / 2 / a;
//    }
//    else if (fabsf(Tb) < Str->Settings.dt)
//    {
//        //是加速到减速阶段中间过程小
//        Sn = Sn + Vm * (Str->Settings.dt - fabsf(Tb)) + Vn * Str->Settings.dt + Accl_Cal / 2 * Str->Settings.dt * Str->Settings.dt;
//        Vn = Vm;
//    }
//    else
//    {
        Sn = Sn + Vn * Str->Settings.dt + Accl_Cal / 2 * Str->Settings.dt * Str->Settings.dt;
        Vn += Accl_Cal * Str->Settings.dt;
//    }


//    if (fabsf(Sn - Str->Input.Now.pos) > Str->Settings.POS_Error_Max)
//    {
//        Sn = Sn > Str->Input.Now.pos + Str->Settings.POS_Error_Max ? Str->Input.Now.pos + Str->Settings.POS_Error_Max : Sn;
//        Sn = Sn < Str->Input.Now.pos - Str->Settings.POS_Error_Max ? Str->Input.Now.pos - Str->Settings.POS_Error_Max : Sn;
//        Vn = (Sn - last_Sn) / Str->Settings.dt;
//    }
    Vn = Vn > Str->Settings.Speed_Max ? Str->Settings.Speed_Max : Vn; // 输入限幅
    Vn = Vn < -Str->Settings.Speed_Max ? -Str->Settings.Speed_Max : Vn;

		Str->NewTemp.Tb = Tb;
		Str->NewTemp.Te = Te;
		Str->NewTemp.Tm = Tm;
		Str->NewTemp.Vm = Vm;
		
#undef Sn
#undef St
#undef Vn
#undef a
} 

// 直接修改规划结果
void SetPlanning_SetOutput(SetPlanning_Str *Str, float PosSet)
{
    Str->Output.pos = PosSet;
}

void Planning_updata(float Now_pos, float Set_spe, float Set_pos, float speed_max, SetPlanning_Str *Str)
{
    static float Last_current = 0;
    Str->Settings.Speed_Max = speed_max;
    Str->Input.Now.pos = Now_pos; // SCAP: pos 对应的就现在的电流
    Str->Input.Now.spe = (Now_pos - Last_current) / Str->Settings.dt;
    Str->Input.Set.pos = Set_pos;
    Str->Input.Set.spe = Set_spe;

    Last_current = Str->Input.Now.spe;
}

/**
 * @brief 一维设定值规划
 * @param Str
 * @param Accl_Max
 * @param dt
 * @param POS_Error_Max
 * @param Speed_Max
 */
void SetPlanning_Init(SetPlanning_Str *Str, SetPlanSettings_Str *Settings)
{
    // 初始化结构体内容
    Str->CalTick = 0;
    Str->Input.Now.pos = 0;
    Str->Input.Now.spe = 0;
    Str->Input.Set.pos = 0;
    Str->Input.Set.spe = 0;
    Str->Output.pos = 0;
    Str->Output.spe = 0; // 输出的设定值速度作为速度环的前馈，而不是直接输入
    Str->Settings.Accl_Max = Settings->Accl_Max;
    Str->Settings.dt = Settings->dt;
    Str->Settings.POS_Error_Max = Settings->POS_Error_Max;
    Str->Settings.Speed_Max = Settings->Speed_Max;
}

/**
 * @brief 设定值规划设定数据初始化
 * @param Settings          设定数据结构体
 * @param Speed_Max         调整过程最大速度
 * @param POS_Error_Max     输出设定值对应的最大Error
 * @param Accl_Max          最大加速度
 * @param Period            计算周期 单位s
 */
void SetPlanSettings_Init(SetPlanSettings_Str *Settings,
                          float Speed_Max,
                          float POS_Error_Max,
                          float Accl_Max,
                          float Period)
{
    Settings->Speed_Max = Speed_Max;
    Settings->POS_Error_Max = POS_Error_Max;
    Settings->Accl_Max = Accl_Max;
    Settings->dt = Period;
}
